/*************************************************************************
	> File Name: control_filter.c
	> Author: xxx
	> Mail: xxx 
	> Created Time: 2022年03月26日 星期六 20时42分59秒
 ************************************************************************/
#include "control_filter.h"
#include <math.h>

float a;
float angle;

float g_faccangle;//加速度
float g_fgyroangle;//陀螺仪角速度
float dt=0.005; //互补滤波控制周期

// 变量定义
#define Kp 100.0f                        // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f                // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f                // 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差
//float Yaw,Pitch,Roll;  //偏航角，俯仰角，翻滚角

/**
 * @brief 互补滤波
 *
 * @param accele 加速度
 * @param gyro 角速度
 * @param dt 运行周期
 * @return float
 */
float complementaryFilter(float accele, float gyro, float dt)
{
    a = 0.98;
    angle = a*(angle+gyro*dt)+(1-a)*accele;
    return angle;
}

/**
 * @brief 角度计算函数
 *
 * @param acceles 加速度xyz轴数据
 * @param gryros 陀螺仪xyz轴数据
 * @param angle 偏转角
 *
 */
void angleCalculate(float *acceles, float *gryros, float *angle)
{
    float x_facc, y_facc, z_facc;

    x_facc = acceles[0]/16384.0;
    y_facc = acceles[1]/16384.0;
    z_facc = acceles[2]/16384.0;

    g_faccangle = atan2(y_facc, z_facc) / 3.14 *180;

    g_fgyroangle = gryros[0]/16.4;

    *angle = complementaryFilter(g_faccangle, g_fgyroangle, dt);

}



void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az, float *angle)
{
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;

        // 测量正常化
        norm = sqrt(ax*ax + ay*ay + az*az);
        ax = ax / norm;                   //单位化
        ay = ay / norm;
        az = az / norm;

        // 估计方向的重力
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

        // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);

        // 积分误差比例积分增益
        exInt = exInt + ex*Ki;
        eyInt = eyInt + ey*Ki;
        ezInt = ezInt + ez*Ki;

        // 调整后的陀螺仪测量
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;

        // 整合四元数率和正常化
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

        // 正常化四元
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;

        angle[1]  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数
        angle[2] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
        angle[3] = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //yaw此处没有价值，注掉
}
